/**
 * a1.c
 */
#include "common.h"
#include <bwio.h>
#include "curses.h"
#include "string.h"
#include <ts7200.h>
#include "evio.h"

// command line interface
static char cmd_buf[CMD_BUF_SIZE+1];
static int cmd_ptr;
static int cmd_x, cmd_y;

// digital clock (m:s.d)
static int dc_ticks;

// maximum loop time
static int loop_max;

// train status
static char tr_speed[81];

// switch status
static char sw_dir[256];

int timer3_ticked() {
  int *vic2_fiq_status = (int*)( VIC2_BASE + FIQ_STATUS_OFFSET );
  return *vic2_fiq_status & TC3OI_MASK;
}

int timer3_read() {
  int *timer3_val = (int*)( TIMER3_BASE + VAL_OFFSET );
  return *timer3_val;
}

void display_dc() {
  setcursorxy(0, 1);
  clearline();
  bwprintf( COM2, "%d%d:%d%d.%d    [ loop max %d ms ] [ sensor max %d ms ]",
      dc_ticks/6000, (dc_ticks/600)%10,
      (dc_ticks/100)%6, (dc_ticks/10)%10,
      dc_ticks%10,
      loop_max, ss_max_response);
}

void update_dc() {
  dc_ticks++;
  if (dc_ticks == 60000) dc_ticks = 0;
  display_dc();
  int *timer3_clear = (int*)( TIMER3_BASE + CLR_OFFSET );
  *timer3_clear = 0x1729;
}

void tr(int train_number, int train_speed) {
  struct io tx_tr;
  // set train speed
  tx_tr.b1 = train_speed; tx_tr.b2 = train_number;
  iobuf_enqueue( &txbuf, tx_tr );
  // track train speed
  tr_speed[train_number] = train_speed;
}

void rv(int train_number) {
  int now = timer4_read();
  struct io tx_tr;
  // stop train now
  tx_tr.b1 = 0; tx_tr.b2 = train_number;
  iobuf_enqueue( &txbuf, tx_tr );
  // reverse in 250 ms
  tx_tr.data = now + 250;
  tx_tr.b1 = 0x0f; tx_tr.b2 = train_number;
  txq_push( &timeq, tx_tr );
  // start again in 500 ms
  tx_tr.data = now + 500;
  tx_tr.b1 = tr_speed[train_number]; tx_tr.b2 = train_number;
  txq_push( &timeq, tx_tr );
}

void sw(int switch_number, char switch_direction) {
  int now = timer4_read();
  struct io tx_tr;
  // set switch
  if (switch_direction == 'S') tx_tr.b1 = TRAIN_SW_STRAIGHT;
  else tx_tr.b1 = TRAIN_SW_CURVED;
  tx_tr.b2 = switch_number;
  iobuf_enqueue( &txbuf, tx_tr );
  // turn off solenoid in 250 ms
  tx_tr.data = now + 250;
  tx_tr.b1 = TRAIN_SOLENOID_OFF; tx_tr.b2 = 0x00;
  txq_push( &timeq, tx_tr );
  // track switch direction
  sw_dir[switch_number] = switch_direction;
}

void wh() {
  ssmod_print(&ss_last);
}

void st(int switch_number) {
  bwprintf( COM2, "%c\r\n", sw_dir[switch_number] );
}

void stop() {
  struct io tx_tr;
  tx_tr.b1 = TRAIN_STOP; tx_tr.b2 = 0x00;
  iobuf_enqueue( &txbuf, tx_tr );
}

void go() {
  struct io tx_tr;
  tx_tr.b1 = TRAIN_GO; tx_tr.b2 = 0x00;
  iobuf_enqueue( &txbuf, tx_tr );
}

void help() {
  bwprintf( COM2, "tr train_number train_speed\r\n" );
  bwprintf( COM2, "\tset the given train to run at the given speed\r\n" );
  bwprintf( COM2, "rv train_number\r\n" );
  bwprintf( COM2, "\treverse the direction of the given train\r\n" );
  bwprintf( COM2, "sw switch_number switch_direction\r\n" );
  bwprintf( COM2, "\tset the given switch to the given position\r\n" );
  bwprintf( COM2, "wh\r\n" );
  bwprintf( COM2, "\tdisplay which sensor last saw the train\r\n" );
  bwprintf( COM2, "st switch_number\r\n" );
  bwprintf( COM2, "\tdisplay the position of the given switch\r\n" );
  bwprintf( COM2, "stop\r\n" );
  bwprintf( COM2, "\temergency stop operation\r\n" );
  bwprintf( COM2, "go\r\n" );
  bwprintf( COM2, "\tresume operation after a stop\r\n" );
  bwprintf( COM2, "clear\r\n" );
  bwprintf( COM2, "\tclear the terminal\r\n" );
}

void clear() {
  setcursorxy(0, 2);
  cleardown();
}

int user_input() {
  int *uart2_flags = (int*)( UART2_BASE + UART_FLAG_OFFSET );
  return *uart2_flags & RXFF_MASK;
}

int parse_input( char* buf, char** cmd, char** a1, char** a2 ) {
  *cmd = strtok(buf, ' ');
  if (*cmd == NULL) return 0;
  *a1 = strtok(NULL, ' ');
  if (*a1 == NULL) return 1;
  *a2 = strtok(NULL, ' ');
  if (*a2 == NULL) return 2;
  return 3;
}

/**
 * Returns:
 * -1 for quit
 *  0 for success
 *  1 for command error
 */
int do_command() {
  char *cmd, *a1, *a2;
  int argc, v1, v2;
  argc = parse_input(cmd_buf, &cmd, &a1, &a2);
  if (cmd == NULL) return 1;
  if (!strcmp(cmd, "tr")) {
    if (argc != 3) {
      bwprintf( COM2, "Usage: tr train_number train_speed\r\n" );
      return 1;
    }
    v1 = atoi(a1); v2 = atoi(a2);
    if (v1 < 1 || v1 > 80) {
      bwprintf( COM2, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    if (v2 < 0 || v2 > 14) {
      bwprintf( COM2, "Error: invalid train speed %d!\r\n", v2 );
      return 1;
    }
    tr(v1, v2);
  } else if (!strcmp(cmd, "rv")) {
    if (argc != 2) {
      bwprintf( COM2, "got %d args, expected 2\r\n", argc );
      bwprintf( COM2, "Usage: rv train_number\r\n" );
      return 1;
    }
    v1 = atoi(a1);
    if (v1 < 1 || v1 > 80) {
      bwprintf( COM2, "Error: invalid train number %d!\r\n", v1 );
      return 1;
    }
    rv(v1);
  } else if (!strcmp(cmd, "sw")) {
    if (argc != 3) {
      bwprintf( COM2, "Usage: sw switch_number switch_direction\r\n" );
      return 1;
    }
    v1 = atoi(a1); 
    if (v1 < 0 || v1 > 255) {
      bwprintf( COM2, "Error: invalid switch number %d!\r\n", v1 );
      return 1;
    }
    if (a2[0] != 'S' && a2[0] != 'C') {
      bwprintf ( COM2, "Error: invalid switch direction %c!\r\n", a2[0] );
      return 1;
    }
    sw(v1, a2[0]);
  } else if (!strcmp(cmd, "wh")) {
    if (argc != 1) {
      bwprintf( COM2, "Usage: wh\r\n" );
      return 1;
    }
    wh();
  } else if (!strcmp(cmd, "st")) {
    if (argc != 2) {
      bwprintf( COM2, "Usage: st switch_number\r\n" );
      return 1;
    }
    v1 = atoi(a1); 
    if (v1 < 0 || v1 > 255) {
      bwprintf( COM2, "Error: invalid switch number %d!\r\n", v1 );
      return 1;
    }
    st(v1);
  } else if (!strcmp(cmd, "stop")) {
    stop();
  } else if (!strcmp(cmd, "go")) {
    go();
  } else if (!strcmp(cmd, "help")) {
    help();
  } else if (!strcmp(cmd, "clear")) {
    clear();
  } else if (!strcmp(cmd, "q")) {
    bwprintf( COM2, "Bye!\r\n" );
    return -1;
  } else {
    bwprintf( COM2, "%s: command not found\r\n", cmd );
    return 1;
  }
  return 0;
}

int is_alpha(unsigned char c) {
  return (c == ' ' || (c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') ||
      (c >= '0' && c <= '9'));
}

int update_shell() {
  int ret = 0;
  unsigned char c = bwgetc( COM2 );
  unsavecursor();
  if (c == '\r') {
    bwprintf( COM2, "\r\n" );
    cmd_buf[cmd_ptr] = '\0';
    ret = do_command();
    if (ret >= 0) {
      bwprintf( COM2, "> " );
      cmd_ptr = 0;
    }
  } else if (c == '\x7f' && cmd_ptr > 0) {
    movecursorxy(-1, 0);
    bwputc( COM2, ' ' );
    movecursorxy(-1, 0);
    cmd_ptr--;
  } else if (is_alpha(c) && cmd_ptr < CMD_BUF_SIZE) {
    bwputc( COM2, c );
    cmd_buf[cmd_ptr++] = c;
  }
  savecursor();
  return ret;
}

void init() {
  // set up COM2 I/O
  bwsetspeed( COM2, 115200 );
  bwsetfifo( COM2, OFF );
  bwprintf( COM2, "a1.elf v (0.)*1 \r\n" );
  bwprintf( COM2, "Evan Stratford\r\n\r\n" );

  bwprintf( COM2, "initing string library..." );
  strinit();
  bwprintf( COM2, "done.\r\n" );
  bwprintf( COM2, "initing I/O library..." );
  evio_init();
  bwprintf( COM2, "done.\r\n" );
  
  // set up COM1 I/O
  bwprintf( COM2, "initing COM1 I/O..." );
  int buf;
  int *uart1_lcrl = (int*)( UART1_BASE + UART_LCRL_OFFSET );
  int *uart1_lcrm = (int*)( UART1_BASE + UART_LCRM_OFFSET );
  int *uart1_lcrh = (int*)( UART1_BASE + UART_LCRH_OFFSET );
  int *uart1_ctlr = (int*)( UART1_BASE + UART_CTLR_OFFSET );
  *uart1_lcrl = 0xbf;
  *uart1_lcrm = 0x0;
  buf = *uart1_lcrh;
  buf |= (STP2_MASK | WLEN_MASK);
  buf &= ~FEN_MASK;
  *uart1_lcrh = buf;
  buf = *uart1_ctlr;
  buf |= UARTEN_MASK;
  *uart1_ctlr = buf;
  bwprintf( COM2, "done.\r\n" );
 
  // mark initial train speeds as 0
  // don't actually send commands; assume that the user is reasonable
  // enough to stop the track first
  bwprintf( COM2, "initing trains..." );
  int i;
  for (i = 1; i <= 80; i++) {
    tr_speed[i] = 0x00;
  }
  bwprintf( COM2, "done.\r\n" );

  bwprintf( COM2, "initing switches..." );
  // set initial switch positions FOR EXISTING SWITCHES ONLY to curved
  // give the controller plenty of time between each command
  for (i = 1; i <= 156; i++) {
    if (i == 19) i = 153;
    struct io tx_tr;
    int now;
    // set switch
    tx_tr.b1 = TRAIN_SW_CURVED; tx_tr.b2 = i;
    now = timer4_read();
    while ( timer4_read() - now < 250 ) ;
    io_transmit( &tx_tr );
    // turn off solenoid
    now = timer4_read();
    while ( timer4_read() - now < 250 ) ;
    tx_tr.b1 = TRAIN_SOLENOID_OFF; tx_tr.b2 = 0x00;
    io_transmit( &tx_tr );
    // track switch direction
    sw_dir[i] = 'C';
    // wait until switch is set and solenoid is off
    bwputc( COM2, '.' );
  }
  bwprintf( COM2, "done.\r\n" );
  
  // set up shell
  cmd_buf[0] = '\0';
  cmd_ptr = 0;
  cmd_x = 0; cmd_y = 0;
  clearscr();
  setscrcolor(FG_BASE + WHITE_OFFSET, BG_BASE + BLACK_OFFSET);
  setcursorxy(0, 2);
  bwprintf( COM2, "> " );
  savecursor();
  
  // set up digital clock
  loop_max = 0;
  dc_ticks = 0;
  display_dc();
  int *timer3_ldr = (int*)( TIMER3_BASE + LDR_OFFSET );
  int *timer3_ctrl = (int*)( TIMER3_BASE + CRTL_OFFSET );
  int *vic2_int_select = (int*)( VIC2_BASE + INT_SELECT_OFFSET );
  int *vic2_int_enable = (int*)( VIC2_BASE + INT_ENABLE_OFFSET );
  *timer3_ldr = TIMER3_COUNTDOWN;
  *timer3_ctrl = ENABLE_MASK | MODE_MASK | CLKSEL_MASK;
  *vic2_int_select = TC3OI_MASK;
  *vic2_int_enable = TC3OI_MASK;
}

void record_loop_time(int time) {
  if (time > loop_max) loop_max = time;
}

int main() {
  init();
  int cmd_status = 0;
  int now, diff;
  for ( ; ; ) {
    now = timer4_read();
    if ( evio_qready() ) evio_q();
    if ( evio_rxready() ) evio_rx();
    if ( evio_txready() ) evio_tx();
    if ( timer3_ticked() ) update_dc();
    if ( ss_needs_poll() ) ss_poll();
    if ( user_input() ) cmd_status = update_shell();
    if ( cmd_status == -1 ) break;
    diff = timer4_read() - now;
    record_loop_time(diff);
  }
  return 0;
}
